﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using uam_fps_game.Gfx;
using Microsoft.Xna.Framework;
using uam_fps_game.Physics;
using Microsoft.Xna.Framework.Input;

namespace uam_fps_game.Scenes
{
    class PhysicsTestNode : SceneNode
    {
        public VisualObject Model { get; protected set; }

        protected RigidBody _rigidBody;

        internal PhysicsTestNode(Scene scene)
            : base(scene)
        {
            _rigidBody = PhysicsSystem.Global.CreateRigidBody();

            //float mass = 1.0f;
            //Matrix inertiaTensor = Matrix.Identity;
            //inertiaTensor.M11 = mass * (0.6f * 0.6f + 1.2f * 1.2f) / 12;
            //inertiaTensor.M22 = mass * (4.5f * 4.5f + 1.2f * 1.2f) / 12;
            //inertiaTensor.M33 = mass * (4.5f * 4.5f + 0.6f * 0.6f) / 12;

            //_rigidBody.Init(mass, inertiaTensor, new Vector3(50, 1, 50), Vector3.Zero);
        }

        internal void SetModel(ModelInstance modelInst)
        {
            Model = modelInst;
            _rigidBody.Init(modelInst.ModelObject.Model, new Vector3(50, 1, 50), Vector3.Zero);
        }

        protected override void Update(float deltaTime, Matrix parentWorldMatrix)
        {
            Vector3 forcePoint = _rigidBody.Position;
            forcePoint += new Vector3(0.0f, 0.5f, 0.0f);

            if (Keyboard.GetState().IsKeyDown(Keys.V))
            {
                //Vector3 force = new Vector3(0.0f, 0.0f, 12.5f);
                Vector3 force = new Vector3(12.5f, 0.0f, 0.0f);
                //_rigidBody.AddForce(ref force);
                _rigidBody.AddForceAtPoint(ref force, ref forcePoint);
            }
            if (Keyboard.GetState().IsKeyDown(Keys.C))
            {
                //Vector3 force = new Vector3(0.0f, 0.0f, -12.5f);
                Vector3 force = new Vector3(-12.5f, 0.0f, 0.0f);
                //_rigidBody.AddForce(ref force);
                _rigidBody.AddForceAtPoint(ref force, ref forcePoint);
            }

            if (Keyboard.GetState().IsKeyDown(Keys.F))
            {
                Vector3 force = new Vector3(0.0f, 0.0f, 12.5f);
                //Vector3 force = new Vector3(12.5f, 0.0f, 0.0f);
                //_rigidBody.AddForce(ref force);
                _rigidBody.AddForceAtPoint(ref force, ref forcePoint);
            }
            if (Keyboard.GetState().IsKeyDown(Keys.G))
            {
                Vector3 force = new Vector3(0.0f, 0.0f, -12.5f);
                //Vector3 force = new Vector3(-12.5f, 0.0f, 0.0f);
                //_rigidBody.AddForce(ref force);
                _rigidBody.AddForceAtPoint(ref force, ref forcePoint);
            }

            Matrix world = Matrix.CreateFromQuaternion(_rigidBody.Orientation);
            
            world.Translation = _rigidBody.Position;

            WorldMatrix = world;
            Model.WorldMatrix = world;
        }
    }
}
